#include "SamplingPlanner.h"
#include <pluginlib/class_list_macros.h>
#include "informedRRT.h"
#include "KDNode.hpp"
#include <algorithm>
#include <nav_msgs/Path.h>

// 将规划器注册为插件
PLUGINLIB_EXPORT_CLASS(global_planner::SamplingPlanner, 
                        nav_core::BaseGlobalPlanner);


namespace global_planner{

/**
 * @brief Construct a new Sampling Planner:: Sampling Planner object
 */
SamplingPlanner::SamplingPlanner(){

}

/**
 * @brief Construct a new Sampling Planner:: Sampling Planner object
 * @param  name             规划器名称
 * @param  costmap_ros      代价地图指针
 */
SamplingPlanner::SamplingPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
    initialize(name, costmap_ros);
}

/**
 * @brief 规划器初始化函数，从基类中重载
 * @param  name             规划器名称
 * @param  costmap_ros      代价地图指针
 */
void SamplingPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
    if(!initialized_){
        costmap_ros_ = costmap_ros;
        costmap_ = costmap_ros_->getCostmap();

        // 初始化规划器参数
        ros::NodeHandle private_nh("~/" + name);
        private_nh.param("step_size", step_size_, costmap_->getResolution());
        private_nh.param("min_dist_from_robot", min_dist_from_robot_, 0.10);
        world_model_ = new base_local_planner::CostmapModel(*costmap_);

        plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);

        initialized_ = true;
    }
    else
        ROS_WARN("This planner has already been initialized... doning nothing");


}

/**
 * @brief 规划路径函数，从基类中重载
 * @param  start            起点
 * @param  goal             终点
 * @param  plan             规划得到的路径
 * @return true 
 * @return false 
 */
bool SamplingPlanner::makePlan( const geometry_msgs::PoseStamped& start,
                                const geometry_msgs::PoseStamped& goal,
                                std::vector<geometry_msgs::PoseStamped>& plan)
{
    boost::mutex::scoped_lock lock(mutex_);
    if(!initialized_){
        ROS_ERROR("This planner has not been initialized yet, but it's being used. Please call initialize() befor use.");
        return false;
    }
    plan.clear();
    frame_id_ = start.header.frame_id;

    /***** 将起点和终点从世界坐标系转换到地图坐标系 *****/

    double wx = start.pose.position.x;
    double wy = start.pose.position.y;

    unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
    double start_x, start_y, goal_x, goal_y;

    if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
        ROS_WARN(
                "The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
        return false;
    }
    worldToMap(wx, wy, start_x, start_y);

    wx = goal.pose.position.x;
    wy = goal.pose.position.y;

    if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
        ROS_WARN_THROTTLE(1.0,
                "The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
        return false;
    }
    worldToMap(wx, wy, goal_x, goal_y);

    // 获取地图尺寸
    int nx = costmap_->getSizeInCellsX();
    int ny = costmap_->getSizeInCellsY();

    informedRRT rrt; // 规划器
    rrt.setSize(nx, ny);
    
    auto startNode = std::make_shared<KDNode>((int)start_x, (int)start_y); // 起点节点
    auto goalNode = std::make_shared<KDNode>((int)goal_x, (int)goal_y);    // 终点节点
    vector<KDNodePtr> path; // 用于保存搜索到的路径
    if(rrt.search(costmap_->getCharMap(), startNode, goalNode, path)){
        for(int i = 0; i < path.size(); i++)
        {
            KDNodePtr temp = path[i];
            // 将路径点从地图坐标系转换到世界坐标系
            mapToWorld(temp->point[X], temp->point[Y], wx, wy);
            geometry_msgs::PoseStamped point = goal;
            point.pose.position.x = wx;
            point.pose.position.y = wy;
            plan.push_back(point);

        }
        // 发布路径话题
        publishPlan(plan);
        return true;
    }
    else{
        ROS_INFO("Can't find path!");
        return false;
    }
}

/**
 * @brief 发布路径点话题
 * @param  path             规划得到的路径
 */
void SamplingPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path) {
    if (!initialized_) {
        ROS_ERROR(
                "This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return;
    }

    //create a message for the plan
    nav_msgs::Path gui_path;
    gui_path.poses.resize(path.size());

    gui_path.header.frame_id = frame_id_;
    gui_path.header.stamp = ros::Time::now();

    // Extract the plan in world co-ordinates, we assume the path is all in the same frame
    for (unsigned int i = 0; i < path.size(); i++) {
        gui_path.poses[i] = path[i];
    }

    plan_pub_.publish(gui_path);
}
/**
 * @brief 从地图坐标系转换到世界坐标系
 * @param  mx               地图x坐标
 * @param  my               地图y坐标
 * @param  wx               世界x坐标的引用
 * @param  wy               世界y坐标的引用
 */
void SamplingPlanner::mapToWorld(double mx, double my, double& wx, double& wy) {
    wx = costmap_->getOriginX() + (mx+convert_offset_) * costmap_->getResolution();
    wy = costmap_->getOriginY() + (my+convert_offset_) * costmap_->getResolution();
}
/**
 * @brief 从世界坐标系转换到地图坐标系
 * @param  wx               世界x坐标
 * @param  wy               世界y坐标
 * @param  mx               地图x坐标的引用
 * @param  my               地图y坐标的引用
 * @return true 
 * @return false 
 */
bool SamplingPlanner::worldToMap(double wx, double wy, double& mx, double& my) {
    double origin_x = costmap_->getOriginX(), origin_y = costmap_->getOriginY();
    double resolution = costmap_->getResolution();

    if (wx < origin_x || wy < origin_y)
        return false;

    mx = (wx - origin_x) / resolution - convert_offset_;
    my = (wy - origin_y) / resolution - convert_offset_;

    if (mx < costmap_->getSizeInCellsX() && my < costmap_->getSizeInCellsY())
        return true;

    return false;
}
}